{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/force.html).  I recommend having both windows open, side-by-side!\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "from pydrake.all import (\n",
    "    DiagramBuilder,\n",
    "    Evaluate,\n",
    "    LogVectorOutput,\n",
    "    Simulator,\n",
    "    SymbolicVectorSystem,\n",
    "    Variable,\n",
    ")\n",
    "\n",
    "q = Variable(\"q\")\n",
    "v = Variable(\"v\")\n",
    "t = Variable(\"t\")\n",
    "kp = 10\n",
    "kd = 1\n",
    "m = 1\n",
    "g = 10\n",
    "c = 10  # amplitude.  set to zero to see the steady-state response.\n",
    "q_d = c * np.sin(t)\n",
    "v_d = c * np.cos(t)\n",
    "a_d = -c * np.sin(t)\n",
    "\n",
    "plt.figure()\n",
    "ts = np.linspace(0, 10, 100)\n",
    "plt.plot(ts, c * np.sin(ts), label=\"desired\")\n",
    "\n",
    "for control in [\"pd\", \"stiffness\", \"inverse_dynamics\"]:\n",
    "    if control == \"pd\":\n",
    "        u = kp * (q_d - q) + kd * (v_d - v)\n",
    "    if control == \"stiffness\":\n",
    "        u = kp * (q_d - q) + kd * (v_d - v) + m * g\n",
    "    if control == \"inverse_dynamics\":\n",
    "        u = m * (a_d + kp * (q_d - q) + kd * (v_d - v)) + m * g\n",
    "\n",
    "    sys = SymbolicVectorSystem(\n",
    "        state=[q, v], time=t, dynamics=[v, -g + u / m], output=[q]\n",
    "    )\n",
    "\n",
    "    builder = DiagramBuilder()\n",
    "    system = builder.AddSystem(sys)\n",
    "    logger = LogVectorOutput(system.get_output_port(0), builder)\n",
    "    diagram = builder.Build()\n",
    "\n",
    "    context = diagram.CreateDefaultContext()\n",
    "    context.SetContinuousState([0.9, 0])\n",
    "\n",
    "    simulator = Simulator(diagram, context)\n",
    "    simulator.AdvanceTo(10)\n",
    "\n",
    "    # Plot the results.\n",
    "    log = logger.FindLog(context)\n",
    "    plt.plot(log.sample_times(), log.data().transpose(), label=control)\n",
    "\n",
    "plt.legend()\n",
    "plt.xlabel(\"t (seconds)\")\n",
    "plt.ylabel(\"z (meters)\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "name": "Robotic Manipulation - Force Control.ipynb",
   "provenance": []
  },
  "interpreter": {
   "hash": "b0fa6594d8f4cbf19f97940f81e996739fb7646882a419484c72d19e05852a7e"
  },
  "kernelspec": {
   "display_name": "Python 3.9.12 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}
